import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.robotics.navigation.TachoPilot;
import Interface.TrackMap;
import behavior.Analyser;
import behavior.AutoPilot;
import behavior.SlopeDetector;

/**
 * atm responsible for constructing an array which holds all the trackPieces. It
 * also makes sure the robot is at a known location before the robot starts
 * analyzing that piece of the track. It assumes that the robot, when done
 * analyzing the current piece, is in the middle of the current piece, facing
 * the exit.
 * 
 */
public class Main {

	private static TachoPilot pilot;
	private static double circ = 5.43; // cm
	private static double axis = 16.18;
	private static final double DEF_CIRC = 5.43;
	private static final double DEF_AXIS = 16.26;
	private static Analyser analyser;
	private static AutoPilot autoPilot;
	private static TrackMap track = new TrackMap();
	//private static ArrayList<Integer> trackList = new ArrayList<Integer>();
	private static SlopeDetector slopeDetector;
	
	
	
	public static void main(String[] args) throws Exception {
//		System.out.println("Want to calibrate?");
//		int b = Button.waitForPress();
//		if (b == Button.ID_ENTER)
//			calibrate();
		
		pilot = new TachoPilot((float) circ, (float) circ, (float) axis, Motor.B, Motor.C,true);
//		slopeDetector = new SlopeDetector(pilot);
//		slopeDetector.Test();
		analyser = new Analyser(pilot);
//		analyser.TestBarcodeAnalyser();
		analyseTrack();
	}

	private static void analyseTrack() {
		int b = Button.waitForPress();
		boolean finish = false;
		while (!finish) {
			int panel = analyser.AnalysePanel();
			//trackList.add(panel);
			track.update(panel);
			System.out.println("Main: " + panel);
			if (track.completeTrack())
				finish = true;
			/*
			 * 1 = straight 6 = right turn 3= left turn
			 */

//			pilot.travel(80);
			// if (panel == 0) {
			// pilot.travel(78);
			// } else if (panel == 1) {
			// pilot.travel(25);
			// } else if (panel == 2) {
			// pilot.travel(25);
			// }
			//b = Button.waitForPress();
		}
		LCD.clear();
		for (int tracknr : track.getMap()) {
			System.out.println(tracknr);
		}
		Button.waitForPress();
		LCD.clear();
		
		
		autoPilot = new AutoPilot(track.getMap(), pilot);
		autoPilot.travel();
		
	}

	//not used anymore
	private static void loadCalibrationData() {		
		LCD.drawString("Use Calibration data?", 0, 0);
		int b = Button.waitForPress();
		boolean load = false;
		try {
			if (b == 1) {
				File f = new File("calib.dat");
				if (!f.exists())
					throw new FileNotFoundException();
				FileInputStream fis = new FileInputStream(f);
				long bits = 0;
				byte[] bytes = new byte[8];
				fis.read(bytes, 0, 8);
				for (int i = 0; i < 8; i++) {
					bits = (bits << 8) + (long) (bytes[i]);
				}
				circ = Double.longBitsToDouble(bits);
				bits = 0;
				fis.read(bytes, 0, 8);
				for (int i = 0; i < 8; i++) {
					bits = (bits << 8) | bytes[i];
				}
				axis = Double.longBitsToDouble(bits);
				bits = 0L;
				byte byt = 0;
				for (int i = 0; i < 8; i++) {
					byt = (byte) (fis.read());
					bits = (bits << 8) + (long) (byt);
				}
				circ = Double.longBitsToDouble(bits);
				bits = 0L;
				for (int i = 0; i < 8; i++) {
					byt = (byte) (fis.read());
					bits = (bits << 8) + (long) (byt);
				}
				axis = Double.longBitsToDouble(bits);
				fis.close();
				LCD.clearDisplay();
//				LCD.drawString("Lo {
//			case 0: pilot.travel(80);
//aded values:", 0, 0);
				LCD.drawString("Circ:" + circ, 0, 1);
				LCD.drawString("Axis:" + axis, 0, 2);
				Button.waitForPress();
				load = true;
			}
		} catch (Exception e) {
			LCD.drawString("Load error.     ", 0, 0, true);
			Button.waitForPress();
		}
		if (!load) {
			circ = DEF_CIRC;
			axis = DEF_AXIS;
		}
	}

	public static void calibrate() {
		pilot = new TachoPilot((float) DEF_CIRC, (float) DEF_AXIS, Motor.B,
				Motor.C);
//		LCD.drawString("Wheel circ.", 0, 0, true);
//		while (true) {
//			pilot.travel(300);
//
//			double distance = 0.0;
//			double increment = 0.1;
//			LCD.drawString("Increment: " + increment + "cm    ", 0, 2);
//			LCD.drawString("Devation: " + distance + "cm        ", 0, 3);
//
//			Buttonloop: {
//				while (true) {
//					int button = Button.waitForPress();
//					switch (button) {
//					case 1:
//						break Buttonloop;
//					case 2:
//						distance -= increment;
//						break;
//					case 4:
//						distance += increment;
//						break;
//					case 8:
//						if (increment < 1)
//							increment = 1.0;
//						else if (increment < 10)
//							increment = 10.0;
//						else
//							increment = 0.1;
//					}
//					LCD.drawString("Increment: " + Double.toString(increment)
//							+ "cm   ", 0, 2);
//					LCD.drawString("Deviation: " + Double.toString(distance)
//							+ "cm         ", 0, 3);
//				}
//			}
//
//			circ = (300.0 + (distance)) / (300.0 / (Main.circ));
//			LCD.drawString("New: " + circ + "         ", 0, 2);
//			pilot = new TachoPilot((float) circ, (float) Main.axis, Motor.B,
//					Motor.C);
//			int b = Button.waitForPress();
//			if (b == Button.ID_ESCAPE)
//				break;
//		}

		LCD.clearDisplay();
		LCD.drawString("Track width", 0, 0, true);
		while (true) {
			pilot.rotate(-3600);

			double angle = 0;
			double increment = 0.1;
			LCD.drawString("Increment: " + increment + "    ", 0, 2);
			LCD.drawString("Devation: " + angle + "        ", 0, 3);

			Buttonloop: {
				while (true) {
					int button = Button.waitForPress();
					switch (button) {
					case 1:
						break Buttonloop;
					case 2:
						angle -= increment;
						break;
					case 4:
						angle += increment;
						break;
					case 8:
						if (increment < 1)
							increment = 1.0;
						else if (increment < 10)
							increment = 10.0;
						else
							increment = 0.1;
					}
					LCD.drawString("Increment: " + Double.toString(increment)
							+ "   ", 0, 2);
					LCD.drawString("Deviation: " + Double.toString(angle)
							+ "         ", 0, 3);
				}
			}

			Main.axis = (Main.axis * 3600.0) / (3600.0 + angle);
			LCD.drawString("New: " + axis + "         ", 0, 2);
			pilot = new TachoPilot((float) Main.circ, (float) axis, Motor.B,
					Motor.C);
			int b = Button.waitForPress();
			if (b == 8)
				break;
		}

		LCD.clearDisplay();
		LCD.drawString("Save calibration?", 0, 0, true);
		int b = Button.waitForPress();
		try {
			if (b == 1) {
				File f = new File("calib.dat");
				if (f.exists())
					f.delete();
				if (!f.createNewFile())
					throw new IOException();
				FileOutputStream fos = new FileOutputStream(f);
				long bits = Double.doubleToLongBits(circ);
				byte[] bytes = new byte[8];
				for (int i = 0; i < 8; i++) {
					// bytes[i] = (byte)((bits>>>((7-i)*8))&0xff);
					bytes[7 - i] = (byte) (bits & 0xff);
					bits = bits >>> 8;
				}
				for (int i = 0; i < 8; i++) {
					fos.write(bytes[i]);
				}
				bits = Double.doubleToLongBits(axis);
				for (int i = 0; i < 8; i++) {
					// bytes[i] = (byte)((bits>>>((7-i)*8))&0xff);
					bytes[7 - i] = (byte) (bits & 0xff);
					bits = bits >>> 8;
				}
				for (int i = 0; i < 8; i++) {
					fos.write(bytes[i]);
				}
				fos.close();
				LCD.drawString("Saved.       ", 0, 0, true);
			}
		} catch (Exception e) {
			LCD.drawString("Save error.     ", 0, 0, true);
		}
		Button.waitForPress();
	}
	
	public static void testStayInMiddle(){
		List<Integer> list = new ArrayList<Integer>();
		list.add(0);
		while(true){
			AutoPilot autoPilot = new AutoPilot(list, pilot);
			autoPilot.travel();
			Button.waitForPress();
		}
		
	}
}
